/*
 * FlightController_Misc.cpp
 *
 *  Created on: Feb 9, 2014
 *      Author: ppks
 */
#include "common.h"
#include "FlightController_Common.h"


/********************************************************************************/
/* Constants Definition                                                         */
/********************************************************************************/


/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
static uint8_t  vbatMin = VBATNOMINAL;  /* lowest battery voltage in 0.1V steps */


/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/


/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/

/*******************************************************************************
* Function Name:
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void FlightController_AnalogSensorsRead(void)
{
	static uint8_t analogReaderTaskOrrder = 0; 	/* query at most one multiplexed analog channel per MWii cycle */

	switch (analogReaderTaskOrrder++ % 3)
	{
#if defined(POWERMETER_HARD)
	case 0:
		uint16_t pMeterRaw; 		/* used for current reading */
		static uint32_t lastRead = currentLoopTimeStamp;
		static uint8_t ind = 0;
		static uint16_t pvec[PSENSOR_SMOOTH], psum;
		uint16_t p =  analogRead(PSENSORPIN);
	#if PSENSOR_SMOOTH != 1
		psum += p;
		psum -= pvec[ind];
		pvec[ind++] = p;
		ind %= PSENSOR_SMOOTH;
		p = psum / PSENSOR_SMOOTH;
	#endif
		powerValue = ( FC_Config.MISC.psensornull > p ? FC_Config.MISC.psensornull - p : p - FC_Config.MISC.psensornull); /* do not use abs(), it would induce implicit cast to uint and overrun */
		analogData.amperage = powerValue * FC_Config.MISC.pint2ma;
		pMeter[PMOTOR_SUM] += ((currentLoopTimeStamp - lastRead) * (uint32_t)((uint32_t)powerValue * FC_Config.MISC.pint2ma)) / 100000; /* [10 mA * msec] */
		lastRead = currentLoopTimeStamp;
		break;
#endif /* POWERMETERs_HARD */

#if defined(VBAT_ENABLED)
	case 1:
		static uint8_t ind = 0;
		static uint16_t vvec[VBAT_SMOOTH];
		static uint16_t vsum;
		uint16_t currentVoltageADC;

		/* Read voltage value from ADC */
		currentVoltageADC = analogRead(V_BATPIN);
		#if VBAT_SMOOTH == 1
			analogData.vbat = (currentVoltageADC << 4) / FC_Config.MISC.vbatscale; /* result is Vbatt in 0.1V steps */
		#else /* VBAT_SMOOTH */
			vsum += currentVoltageADC;
			vsum -= vvec[ind];
			vvec[ind++] = currentVoltageADC;
			ind %= VBAT_SMOOTH;

			#if VBAT_SMOOTH == 16
				analogData.vbat = vsum / FC_Config.MISC.vbatscale; /* result is Vbatt in 0.1V steps */
			#elif VBAT_SMOOTH < 16
				analogData.vbat = (vsum * (16 / VBAT_SMOOTH)) / FC_Config.MISC.vbatscale; /* result is Vbatt in 0.1V steps */
			#else
				analogData.vbat = ((vsum / VBAT_SMOOTH) * 16) / FC_Config.MISC.vbatscale; /* result is Vbatt in 0.1V steps */
			#endif /* VBAT_SMOOTH */
		#endif /* VBAT_SMOOTH */
		break;
#endif /* VBAT_ENABLED */

#if defined(RX_RSSI)
	case 2:
	{
		static uint8_t ind = 0;
		static uint16_t rvec[RSSI_SMOOTH];
		static uint16_t rsum;
		uint16_t r = analogRead(RX_RSSI_PIN);

		#if RSSI_SMOOTH == 1
			analogData.rssi = r;
		#else /* RSSI_SMOOTH */
			rsum += r;
			rsum -= rvec[ind];
			rvec[ind++] = r;
			ind %= RSSI_SMOOTH;
			r = rsum / RSSI_SMOOTH;
			analogData.rssi = r;
		#endif /* RSSI_SMOOTH */
		break;
	}
#endif /* defined(RX_RSSI) */
	} /* end of switch() */
}


